#include <opencv2/opencv.hpp>
//#include <iostream>
#include <math.h>
#include "windmill.hpp"
#include "getNeeds.h"

using namespace std;
using namespace cv;

//在拟合的时候取的点的数目，用变量储存便于调试时统一更改
int num=1000;

//起始画布
Mat src;

//计次器，用于在调试时查看程序运行的次数
int counter1=0;

int main() {
    //getNeeds类中存放着所有需要用到的函数，建立一个getNeeds类以进行函数的调用
    getNeeds a;
    //获取当前的时间，因为该时间过大，在之后的程序中使t_start时刻作为程序中的零时刻
    const int t_start=std::chrono::system_clock::now().time_since_epoch().count() / 1e9;
    //初始化一个能量机关，此时为零时刻
    WINDMILL::WindMill vm(0);
    //创建记录能量机关中心点以及待击打模块中心点x、y值的变量，因为在之后需要计算速度，需要用到前后两组数据，故建立pre组用于储存当前的上一批的变量
    int center_x = 0,center_y = 0,target_x = 0,target_y = 0,pre_center_x=0,pre_center_y = 0,pre_target_x = 0,pre_target_y = 0;
    //创建记录相位alpha的变量alpha，在多次记录之后认为其值应在1.8左右，初始化为1.8以防止其在程序尚在获取拟合所需数据时记录能量机关旋转速度的变量v1
    double alpha=1.8,v1;
    //创建两个记录时间的变量，用于计算速度等
    double t0 ,pre_t0,r;
    //创建两个数组，用于储存用于拟合的点，可理解为横坐标为时间time1，纵坐标为sin1，是sin(1.884t+alpha)的实际值，用(v-1.305)/0.785求出
    double sin1[num],time1[num];
    //记次器归零
    counter1=0;
    //初始化一轮，以确保在后续运算中pre组的值不为空，具体函数作用在后面while中统一解释
    //初始化开始
    t0 = std::chrono::system_clock::now().time_since_epoch().count() / 1e9 - t_start;
    src=vm.getMat(t0);
    a.getCoordinate(src,center_x,center_y,target_x,target_y);
    pre_center_x=center_x,pre_center_y=center_y,pre_target_x=target_x,pre_target_y=target_y,pre_t0=t0;
    imshow("b",src);
    waitKey(1);
    //初始化结束
    //正式开始循环
    while(1)
    {
        //获取以t_start为初始时刻的时间
        t0 = std::chrono::system_clock::now().time_since_epoch().count() / 1e9-t_start;
        //获取当前的能量机关图片
        src=vm.getMat(t0);
        //获取能量机关中心点及待击打板块中心点坐标，输入为src，用四个引用修改实参实现返回四个值，详情见getNeeds.cpp
        a.getCoordinate(src,center_x,center_y,target_x,target_y);
        //获取当前能量机关旋转速度以及能量机关的半径，输入为十个数值，分别是击打板中心点坐标xy、能量机关中心点坐标xy、上一循环中击打板中心点坐标xy、上一循环中能量机关中心点坐标xy、当前时间、上一循环中当前时间，使用引用修改实参以获得能量机关半径r，返回值为能量机关旋转速度，详情见getNeeds.cpp
        v1=a.getVreal(target_x,target_y,center_x,center_y,pre_target_x,pre_target_y,pre_center_x,pre_center_y,t0,pre_t0,r);
        //将当前数据存入pre组，等待下一循环可调用
        pre_center_x=center_x,pre_center_y=center_y,pre_target_x=target_x,pre_target_y=target_y,pre_t0=t0;
        //向sin1与time1中写入num个当前速度以及时刻作为拟合的点
        if(counter1<num&&counter1>-1)
            sin1[counter1]=v1,time1[counter1]=t0,counter1++,cout<<counter1<<endl;
        //录入完毕之后进行拟合以获取alpha，详情见getNeeds.cpp
        if(counter1==num)
            alpha=a.getAlpha(sin1,time1),counter1=-1,cout<<alpha<<endl;
        //计算后求出的角的积分
        double fangle = 1.305 * t0 - (0.785 / 1.884) *  cos(1.884 * t0 + alpha);
        //绘制拟合积分计算之后获得的点
        circle(src, Point2f (center_x+cos(fangle)*r,center_y-sin(fangle)*r), 5, Scalar(255, 0, 0), -1, 8);
        //显示图片
        imshow("b",src);
        waitKey(1);
    }
}